#include "pid_controller.h"
#include "main.h"

int open_loop = 0;          //是否开环控制，默认为否
double open_loop_duty_cycle = 0.0;  //开环控制的输出
double target_speed = -1;
double Kp = -1;
double Ki = -1;
double Kd = -1;

static double integral = 0.0;
static double derivative = 0.0;
static double error_last = 0.0;

//根据当前速度，计算出需要输出的PWM占空比:百分之output
double pid_controller(double current_speed)
{
    //开环：直接输出设定的占空比。闭环：根据转速，计算输出占空比
    if(open_loop)
        return open_loop_duty_cycle;
    double error = target_speed - current_speed;
    integral += error;
    derivative = error - error_last;
    double output = Kp * error + Ki * (SAMPLE_PERIOD / 1000.0 / 1000.0) * integral + Kd / (SAMPLE_PERIOD / 1000.0 / 1000.0) * derivative;
    error_last = error;
    if (output < 0.0)
        output = 0.0;
    else if (output > 100.0)
        output = 100.0;
    return output;
}

//设置目标速度
void set_target_speed(double speed_rpm)
{
    if(speed_rpm < 0.0)
    {
        target_speed = 0.0;
        return;
    }
    if(speed_rpm > 320)
    {
        target_speed = 320.0;
        return;
    }
    target_speed = speed_rpm;
}



//重置积分，微分，误差
void reset_controller_state()
{
    derivative = integral = error_last = 0.0;
}
